/**
 * @file sensor_noise.cpp
 * @author Lusheng
 * @解释
 * @version 0.1
 * @date 2025-03-18
 *
 * @copyright Copyright (c) 2025
 *
 */
#include "sensor_noise.h"
#include <chrono>
#include <iostream>


namespace sensor {

// NoiseGenerator实现
NoiseGenerator::NoiseGenerator() : noiseType(NoiseType::NONE),
                                   noiseAmplitude(0.0)
{
    // 使用当前时间初始化随机数生成器
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    generator = std::mt19937(seed);

    // 初始化分布
    normalDist = std::normal_distribution<double>(0.0, 1.0);
    uniformDist = std::uniform_real_distribution<double>(-1.0, 1.0);
}


void NoiseGenerator::configureNoise(NoiseType type, double amplitude)
{
    noiseType = type;
    noiseAmplitude = amplitude;
}


double NoiseGenerator::applyNoise(double sensorValue)
{
    double noise = 0.0;

    switch (noiseType) {
    case NoiseType::GAUSSIAN:
        noise = normalDist(generator) * noiseAmplitude;
        break;
    case NoiseType::UNIFORM:
        noise = uniformDist(generator) * noiseAmplitude;
        break;
    case NoiseType::NONE:
    default:
        return sensorValue; // 不添加噪声
    }

    return sensorValue + noise;
}

// NoiseManager 实现
NoiseManager &NoiseManager::getInstance()
{
    static NoiseManager instance;
    return instance;
}

void NoiseManager::initialize(const mjModel *model)
{
    if (!model) {
        std::cerr << "Error: Cannot initialize NoiseManager with null model" << std::endl;
        return;
    }

    int nsensor = model->nsensor;
    noiseGenerators.resize(nsensor);

    // 默认配置 - 可以根据XML中定义的噪声参数进行设置
    for (int i = 0; i < nsensor; i++) {
        // 读取XML中定义的噪声振幅
        double noiseAmp = model->sensor_noise[i];
        if (noiseAmp > 0) {
            noiseGenerators[i].configureNoise(NoiseType::GAUSSIAN, noiseAmp);
        }
    }

    initialized = true;
}

void NoiseManager::configureSensor(int sensorIndex, NoiseType type, double amplitude)
{
    if (!initialized) {
        std::cerr << "Error: NoiseManager not initialized" << std::endl;
        return;
    }

    if (sensorIndex >= 0 && sensorIndex < noiseGenerators.size()) {
        noiseGenerators[sensorIndex].configureNoise(type, amplitude);
    } else {
        std::cerr << "Error: Invalid sensor index: " << sensorIndex << std::endl;
    }
}

double NoiseManager::applySensorNoise(int sensorIndex, double value)
{
    if (!initialized || sensorIndex < 0 || sensorIndex >= noiseGenerators.size()) {
        return value;
    }

    return noiseGenerators[sensorIndex].applyNoise(value);
}

// 新增方法：应用噪声到所有传感器
void NoiseManager::applyNoiseToAllSensors(const mjModel* m, mjData* d)
{
    auto sensorCount = m->nsensor;

    if (!initialized || !m || !d) {
        return;
    }
    
    // 为每个传感器应用噪声
    for (int i = 0; i < sensorCount; i++) {
        // 获取原始传感器数据
        double originalValue = d->sensordata[i];
        
        // 应用噪声
        d->sensordata[i] = applySensorNoise(i, originalValue);
    }
}

// 新增方法：记录噪声应用情况
void NoiseManager::logNoiseApplication(const mjModel* m, mjData* d, bool verbose)
{
    if (!m || !d || !initialized) {
        return;
    }
    
    // 只记录前几个传感器（防止日志过多）
    int maxSensorsToLog = verbose ? m->nsensor : std::min(2, m->nsensor);
    
    for (int i = 0; i < maxSensorsToLog; i++) {
        const char* sensorName = mj_id2name(m, mjOBJ_SENSOR, i);
        std::cout << "Sensor " << i << (sensorName ? " (" + std::string(sensorName) + ")" : "") 
                  << ": value=" << d->sensordata[i] << std::endl;
    }
}

} // namespace sensor
